import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import TimerAction, GroupAction
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import PushRosNamespace

def generate_launch_description():
    # 获取包共享目录
    bringup_dir = get_package_share_directory('rm_bringup')

    # 加载 launch 参数
    launch_params_first_path = os.path.join(
        bringup_dir, 'config', 'node_params_first', 'launch_params.yaml')
    with open(launch_params_first_path, 'r') as file:
        launch_params_first = yaml.safe_load(file)

    # 定义 get_params 函数
    def get_params(config_dir, name):
        params_path = os.path.join(bringup_dir, 'config', config_dir, f'{name}_params.yaml')
        return params_path

    # 定义 get_camera_detector_container 函数
    def get_camera_detector_container(image_node, detector_node):
        nodes_list = [detector_node, image_node]
        container = ComposableNodeContainer(
            name='camera_detector_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container_mt',
            composable_node_descriptions=nodes_list,
            output='both',
            emulate_tty=True,
            ros_arguments=['--ros-args', ],
        )
        return TimerAction(
            period=2.0,
            actions=[container],
        )

    # 图像
    if launch_params_first['video_play']: 
        image_node_first = ComposableNode(
            package='mas_hik_camera_first',
            plugin='fyt::camera_driver::VideoPlayerNode',
            name='video_player',
            parameters=[get_params('node_params_first', 'video_player')],
            extra_arguments=[{'use_intra_process_comms': True}]
        )
    else:
        image_node_first = ComposableNode(
            package='mas_hik_camera_first',
            plugin='hik_camera::HikCameraNode',
            name='camera_driver',
            parameters=[get_params('node_params_first', 'camera_driver')],
        )

    # 串口
    serial_driver_node_first = Node(
        package='mas_serial_driver_first',
        executable='mas_serial_driver_first_node',
        name='serial_driver',
        output='both',
        emulate_tty=True,
        parameters=[get_params('node_params_first', 'serial_driver')],
        ros_arguments=['--ros-args', ],
    )
        
    # 装甲板识别
    armor_detector_node_first = ComposableNode(
        package='armor_detector', 
        plugin='fyt::auto_aim::ArmorDetectorNode',
        name='armor_detector',
        parameters=[get_params('node_params_first', 'armor_detector')],
        extra_arguments=[{'use_intra_process_comms': True}]
    )
    
    # 装甲板解算
    armor_solver_node_first = Node(
        package='armor_solver',
        executable='armor_solver_node',
        name='armor_solver',
        output='both',
        emulate_tty=True,
        parameters=[get_params('node_params_first', 'armor_solver')],
        ros_arguments=[],
    )

    # 延迟启动
    delay_serial_node_first = TimerAction(
        period=1.5,
        actions=[serial_driver_node_first],
    )

    delay_armor_solver_node_first = TimerAction(
        period=2.0,
        actions=[armor_solver_node_first],
    )
    
    cam_detector_node_first = get_camera_detector_container(
        image_node=image_node_first,
        detector_node=armor_detector_node_first
    )

    delay_cam_detector_node_first = TimerAction(
        period=2.0,
        actions=[cam_detector_node_first],
    )
    
    # 定义第一个命名空间下的节点
    launch_description_list = [
        PushRosNamespace(launch_params_first['namespace']),
        delay_serial_node_first,
        delay_cam_detector_node_first,
        delay_armor_solver_node_first
    ]
    
    return LaunchDescription(launch_description_list)
